#include "MeVtkMath.h"


vtkSmartPointer<vtkMatrix4x4> MeVtkMath::rotateX(double angle)
{
    vtkSmartPointer<vtkMatrix4x4> m = vtkMatrix4x4::New();
    double ca = cos(angle);
    double sa = sin(angle);

    m->Element[0][0] = 1;
    m->Element[1][0] = 0;
    m->Element[2][0] = 0;
    m->Element[3][0] = 0;

    m->Element[0][1] = 0;
    m->Element[1][1] = ca;
    m->Element[2][1] = sa;
    m->Element[3][1] = 0;

    m->Element[0][2] = 0;
    m->Element[1][2] = -sa;
    m->Element[2][2] = ca;
    m->Element[3][2] = 0;

    m->Element[0][3] = 0;
    m->Element[1][3] = 0;
    m->Element[2][3] = 0;
    m->Element[3][3] = 1;

    m->Modified();

    return m;
}

vtkSmartPointer<vtkMatrix4x4> MeVtkMath::rotateY(double angle)
{
    vtkSmartPointer<vtkMatrix4x4> m = vtkMatrix4x4::New();
    double ca = cos(angle);
    double sa = sin(angle);

    m->Element[0][0] = ca;
    m->Element[1][0] = 0;
    m->Element[2][0] = -sa;
    m->Element[3][0] = 0;

    m->Element[0][1] = 0;
    m->Element[1][1] = 1;
    m->Element[2][1] = 0;
    m->Element[3][1] = 0;

    m->Element[0][2] = sa;
    m->Element[1][2] = 0;
    m->Element[2][2] = ca;
    m->Element[3][2] = 0;

    m->Element[0][3] = 0;
    m->Element[1][3] = 0;
    m->Element[2][3] = 0;
    m->Element[3][3] = 1;

    m->Modified();

    return m;
}

vtkSmartPointer<vtkMatrix4x4> MeVtkMath::rotateZ(double angle)
{
    vtkSmartPointer<vtkMatrix4x4> m = vtkMatrix4x4::New();
    double ca = cos(angle);
    double sa = sin(angle);

    m->Element[0][0] = ca;
    m->Element[1][0] = sa;
    m->Element[2][0] = 0;
    m->Element[3][0] = 0;

    m->Element[0][1] = -sa;
    m->Element[1][1] = ca;
    m->Element[2][1] = 0;
    m->Element[3][1] = 0;

    m->Element[0][2] = 0;
    m->Element[1][2] = 0;
    m->Element[2][2] = 1;
    m->Element[3][2] = 0;

    m->Element[0][3] = 0;
    m->Element[1][3] = 0;
    m->Element[2][3] = 0;
    m->Element[3][3] = 1;

    m->Modified();

    return m;
}

/// x,y,z 传入的是弧度脚, 偏移量
vtkSmartPointer<vtkMatrix4x4> MeVtkMath::rotateZYX(double xInRadian, double yInRadian, double zInRadian)
{
    vtkSmartPointer<vtkMatrix4x4> m = vtkMatrix4x4::New();

    double z = zInRadian;
    double y = yInRadian;
    double x = xInRadian;
    /**
    x,y,z 表示旋转角度
    [ cy*cz, sx*sy*cz - cx*sz, cx*sy*cz + sx*sz]
    [ cy*sz, sx*sy*sz + cx*cz, cx*sy*sz - sx*cz]
    [   -sy,            sx*cy,            cx*cy]
     */

    double cz = cos(z);
    double sz = sin(z);
    double cy = cos(y);
    double sy = sin(y);
    double cx = cos(x);
    double sx = sin(x);

    m->Element[0][0] = cy * cz;
    m->Element[1][0] = cy * sz;
    m->Element[2][0] = -sy;
    m->Element[3][0] = 0;

    m->Element[0][1] = sx*sy*cz - cx*sz;
    m->Element[1][1] = sx*sy*sz + cx*cz;
    m->Element[2][1] = sx * cy;
    m->Element[3][1] = 0;

    m->Element[0][2] = cx*sy*cz + sx*sz;
    m->Element[1][2] = cx*sy*sz - sx*cz;
    m->Element[2][2] = cx * cy;
    m->Element[3][2] = 0;

    m->Element[0][3] = 0;
    m->Element[1][3] = 0;
    m->Element[2][3] = 0;
    m->Element[3][3] = 1;

    m->Modified();

    return m;
}

Eigen::Vector3d MeVtkMath::multiplyPoint(vtkMatrix4x4 *m, Eigen::Vector3d &p)
{
    Eigen::Vector3d outP;
    double *elem = m->GetData();

    double &v1 = p[0];
    double &v2 = p[1];
    double &v3 = p[2];

    outP[0] = v1 * elem[0] + v2 * elem[1] + v3 * elem[2] + elem[3];
    outP[1] = v1 * elem[4] + v2 * elem[5] + v3 * elem[6] + elem[7];
    outP[2] = v1 * elem[8] + v2 * elem[9] + v3 * elem[10] + elem[11];

    return outP;
}

Eigen::Vector3d MeVtkMath::multiplyPoint(vtkMatrix4x4 *m, double p[])
{
    Eigen::Vector3d outP;
    double *elem = m->GetData();

    double &v1 = p[0];
    double &v2 = p[1];
    double &v3 = p[2];

    outP[0] = v1 * elem[0] + v2 * elem[1] + v3 * elem[2] + elem[3];
    outP[1] = v1 * elem[4] + v2 * elem[5] + v3 * elem[6] + elem[7];
    outP[2] = v1 * elem[8] + v2 * elem[9] + v3 * elem[10]+ elem[11];

    return outP;
}
